#include<iostream>
using namespace std;
#include<pcl-1.12/pcl/io/pcd_io.h>
#include<pcl-1.12/pcl/point_types.h>

int main(){
    pcl::PCDReader reader;
    reader.read("out.pcd", *cloud);
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("x");
    pass.setFilterLimits(0.0, 1.0);
    pass.filter(*cloud_filtered);
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("y");
    pass.setFilterLimits(0.0, 1.0);
    pass.filter(*cloud_filtered);
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0.0, 1.0);
    pass.filter(*cloud_filtered);

    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
     sor.setInputCloud(cloud);
    sor.setLeafSize(0.01f, 0.01f, 0.01f);//网格的长宽高
    sor.filter(*cloud_filtered);

    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud(cloud);
    sor.setMeanK(50);
    sor.setStddevMulThresh(1.0);
    sor.filter(*cloud_filtered);

    pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
    outrem.setInputCloud(cloud);
    outrem.setRadiusSearch(0.8);
    outrem.setMinNeighborsInRadius(2);
    outrem.filter(*cloud_filtered);

    //进行平面分割
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients(true);//选择模型系数是否需要优化
    seg.setModelType(pcl::SACMODEL_PLANE);//设置平面模型
    seg.setMethodType(pcl::SAC_RANSAC);//使用RANSAC算法
    seg.setDistanceThreshold(0.01);//容差范围0.01m
    seg.setInputCloud(cloud);
    seg.segment(*inliers, *coefficients);//得到模型中的点的索引和模型参数
    //找到最大平面
    pcl::PointCloud<pcl::PointXYZ> cloud_max;
    pcl::copyPointCloud<PointT>(*cloud, inliers, *cloud_in);
    pcl::io::savePCDFile("./build/RANSAC_building_1.pcd", *cloud_in);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);//以kdtree作为索引方式
    tree->setInputCloud(cloud_filtered);
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance(0.02); //搜索半径,太小了会将一个物体当成多个,太大会将多个物体当成一个
    ec.setMinClusterSize(100);//最少100个点
    ec.setMaxClusterSize(25000);//最多25000个点
    ec.setSearchMethod(tree);
    ec.setInputCloud(cloud_filtered);
    ec.extract(cluster_indices);
    pcl::copyPointCloud<PointT>(*cloud, cluster_inliers, *cloud_cluster_in);
    pcl::io::savePCDFile("./build/cluster_indices.pcd", *cloud_cluste_in);

    return 0;
}
